from maix import camera, display, image, time, app, touchscreen, uart, pinmap
import cv2
import numpy as np
import struct
import gc

# 80 fps or 60fps
cam = camera.Camera(320, 240, fps=60)
cam.constrast(60)
cam.saturation(50)
img = image.Image(320, 240)
img_copy = img
disp = display.Display()
ts = touchscreen.TouchScreen()

devices = uart.list_devices()
serial0 = uart.UART(devices[0], 115200)
pinmap.set_pin_function("A18", "UART1_RX")
pinmap.set_pin_function("A19", "UART1_TX")
device1 = "/dev/ttyS1"
serial1 = uart.UART(device1, 115200)

find_center_method = 1  # 1, 2
area_threshold = 80
pixels_threshold = 50
# thresholds = [[39, 59, 46, 66, 20, 40]]        # red
thresholds = [[65, 85, -28, -8, 66, 86]]        # yellow
# thresholds = [[0, 80, -120, -10, 0, 30]]        # green
# thresholds = [[0, 80, 30, 100, -120, -60]]    # blue

# threshold_blue = [[45, 65, -23, -3, -39, -19]] # 机械臂的识别阈值
# threshold_blue = [[41, 61, -18, 2, -50, -30]] # 机械臂识别早上的识别阈值
# threshold_blue = [[30, 50, 6, 26, -69, -49]] # pc测试
################################################################################################
# 蓝
# L_min = 41
# L_max = 61
# A_min = -20
# A_max = 0
# B_min = -41
# B_max = -21 
################################################################################################
# 红
L_min = 34
L_max = 73
A_min = 39
A_max = 71
B_min = 21
B_max = 41 
binary_flag = 0
threshold_blue = [[L_min, L_max, A_min, A_max, B_min, B_max]]

threshold_jxb = []
threshold_jxb_in = [53, 73, -23, -3, -44, -24]
threshold_jxb_out = [55, 75, -22, -2, -48, -28]
threshold_jxb.append(threshold_jxb_in)
threshold_jxb.append(threshold_jxb_out)

area_threshold2 = 300
pixels_threshold2 = 300
threshold_black = [2, 22, -10, 10, -11, 9]
threshold_white = [65, 85 , -13, 7, -10, 10]
threshold_white = [threshold_white]
threshold_black = [threshold_black]
threshold_distance = 10
white_circle = [(0, 0)] * 5
black_circle = [(0, 0)] * 5
white_flag2 = False
black_flag2 = False
white_x1 = [0] * 5
white_y1 = [0] * 5
black_x1 = [0] * 5
black_y1 = [0] * 5
x1 = [0] * 5
y1 = [0] * 5
r1 = [0] * 5
last_y1 = [0] * 5

x2 = [0] * 5
y2 = [0] * 5
r2 = [0] * 5
last_y2 = [0] * 5
count_white = 0
count_black = 0
valid_flag1 = False
valid_flag2 = False

rect_flag = False
rect_flag2 = False
rect_flag_mode3 = False
rect = np.zeros((9, 2), dtype="float32")
# last_rect = np.zeros((9, 2), dtype="float32")
last_centers = [(0, 0)] * 9
rect_x = [0] * 9
rect_y = [0] * 9
count = 0
rect_x_mode3 = [0] * 9
rect_y_mode3 = [0] * 9

coners_x = []
coners_y = []
jxb_x = [0]
jxb_y = [0]

max_ID=[-1,-1]
XL=0
XR=0
YU=0
YD=0

task1_uart_flag = False
mode = 1 # 初始化模式一
receive_data = None

def collect_threshold(num_iterations):
    apple_thresholds = []
    for i in range(num_iterations):
        img = cam.read()
        img.draw_rect(105, 75, 30, 30, image.COLOR_RED, thickness=2)
        Statistics = img.get_statistics(roi=[105, 75, 30, 30])
        color_l_mode = Statistics.l_mode()
        color_a_mode = Statistics.a_mode()
        color_b_mode = Statistics.b_mode()
        thresholds = (color_l_mode-20,color_l_mode+20,
                            color_a_mode-20,color_a_mode+20,
                            color_b_mode-20,color_b_mode+20)
        apple_thresholds.append(thresholds)                   
    avg_l_min = sum(x[0] for x in apple_thresholds) / num_iterations
    avg_l_max = sum(x[1] for x in apple_thresholds) / num_iterations
    avg_a_min = sum(x[2] for x in apple_thresholds) / num_iterations
    avg_a_max = sum(x[3] for x in apple_thresholds) / num_iterations
    avg_b_min = sum(x[4] for x in apple_thresholds) / num_iterations
    avg_b_max = sum(x[5] for x in apple_thresholds) / num_iterations
    apple_thresholds = [int(avg_l_min), int(avg_l_max), int(avg_a_min), int(avg_a_max), int(avg_b_min), int(avg_b_max)]
    return apple_thresholds

def screen_lab():
    global L_min, L_max, A_min, A_max, B_min, B_max, binary_flag
    global t, threshold_blue
    data = serial0.read()
    if data:
        try:
            # print(data)
            data = data.decode("utf-8")
            # print(data)
            parts = data.split("\r\n")

            for part in parts:
                try:
                    value = int(part)
                    L_min, L_max, A_min, A_max, B_min, B_max, binary_flag = L_max, A_min, A_max, B_min, B_max, binary_flag, value
                    threshold_blue = [[L_min, L_max, A_min, A_max, B_min, B_max]]
                except ValueError:
                    # print("接收错误")
                    pass
            
            # 打印最新的两个数据
            print(f"L_min: {L_min}, L_max: {L_max}, A_min: {A_min}, A_max: {A_max}, B_min: {B_min}, B_max: {B_max}, binary_flag: {binary_flag}")
            print(f"机械臂颜色阈值: {threshold_blue}")

        except ValueError as e:
            print(f"Error decoding hexadecimal data: {e}")  

def is_in_button(x, y, btn_pos):
    return x > btn_pos[0] and x < btn_pos[0] + btn_pos[2] and y > btn_pos[1] and y < btn_pos[1] + btn_pos[3]
def the_exit_button():
    exit_label = "Exit>"
    size = image.string_size(exit_label)
    exit_btn_pos = [300, 0, 5*2 + size.width(), 5 * 2 + size.height()] 
    # draw exit button
    img_copy.draw_string(270, 10, exit_label, image.COLOR_WHITE)
    # 图像按键坐标映射到屏幕上的坐标
    exit_btn_disp_pos = image.resize_map_pos(img.width(), img.height(), disp.width(), disp.height(), image.Fit.FIT_CONTAIN, exit_btn_pos[0], exit_btn_pos[1], exit_btn_pos[2], exit_btn_pos[3])
    x, y, pressed = ts.read()
    if is_in_button(x, y, exit_btn_disp_pos):
        app.set_exit_flag(True)

def key_clicked(btn_rects):
    global last_pressed
    x, y, pressed = ts.read()
    if pressed:
        for i, btn in enumerate(btn_rects):
            if is_in_button(x, y, btn):
                if not last_pressed:
                    last_pressed = True
                    return True, i, btn
    else:
        last_pressed = False
    return False, 0, []

def check_mode_switch(img : image.Image, disp_w, disp_h):
    global mode
    btns = [
        [0, 0, 100, 40],
    ]
    btn_rects_disp = [image.resize_map_pos(img.width(), img.height(), disp_w, disp_h, image.Fit.FIT_CONTAIN, btns[0][0], btns[0][1], btns[0][2], btns[0][3])]
    clicked, idx, rect = key_clicked(btn_rects_disp)
    if clicked:
        mode += 1
        if mode > 3:
            mode = 1
    img_copy.draw_string(2, 10, f"mode {mode}", color=image.COLOR_WHITE, scale=1)
    # img.draw_rect(btns[0][0], btns[0][1], btns[0][2], btns[0][3], image.COLOR_WHITE, 2)

def find_max(blobs):
    max_size=[0,0]
    max_ID=[-1,-1]
    for i in range(len(blobs)):
        if blobs[i].pixels()>max_size[0]:
            max_ID[1]=max_ID[0]
            max_size[1]=max_size[0]
            max_ID[0]=i
            max_size[0]=blobs[i].pixels()
        elif blobs[i].pixels()>max_size[1]:
            max_ID[1]=i
            max_size[1]=blobs[i].pixels()
    return max_ID

def find_max_blob(blobs):
    """
    找到最大的色块并返回其索引
    """
    max_area = 0
    max_index = -1
    for i, blob in enumerate(blobs):
        if blob.pixels() > max_area:
            max_area = blob.pixels()
            max_index = i
    return max_index

def find_grid_centers(corners):
    """
    corners: List of 4 tuples representing the corners of the 3x3 grid in the order:
             top-left, top-right, bottom-right, bottom-left
    """
    # Define the 3x3 grid points in the normalized coordinate space
    grid_points = np.array([[0, 0], [1/3, 0], [2/3, 0], [1, 0],
                            [0, 1/3], [1/3, 1/3], [2/3, 1/3], [1, 1/3],
                            [0, 2/3], [1/3, 2/3], [2/3, 2/3], [1, 2/3],
                            [0, 1], [1/3, 1], [2/3, 1], [1, 1]])

    # Convert corners to numpy array
    src_points = np.array(corners, dtype=np.float32)

    # Define destination points for the perspective transform
    dst_points = np.array([[0, 0], [1, 0], [1, 1], [0, 1]], dtype=np.float32)

    # Compute the perspective transform matrix
    transform_matrix = cv2.getPerspectiveTransform(dst_points, src_points)

    # Transform grid points to image coordinate space
    transformed_points = cv2.perspectiveTransform(np.array([grid_points], dtype=np.float32), transform_matrix)[0]

    # Calculate the center points of the grid cells
    centers = []
    for i in range(0, 3):
        for j in range(0, 3):
            cx = (transformed_points[i * 4 + j][0] + transformed_points[i * 4 + j + 1][0] + 
                  transformed_points[(i + 1) * 4 + j][0] + transformed_points[(i + 1) * 4 + j + 1][0]) / 4
            cy = (transformed_points[i * 4 + j][1] + transformed_points[i * 4 + j + 1][1] + 
                  transformed_points[(i + 1) * 4 + j][1] + transformed_points[(i + 1) * 4 + j + 1][1]) / 4
            centers.append((int(cx), int(cy)))

    return centers

cam.skip_frames(30)           # 跳过开头的30帧
while not app.need_exit():
    t = time.ticks_ms()
    img = cam.read()
    img_copy = img # 复制一份图像，防止干扰
    check_mode_switch(img, disp.width(), disp.height())
    the_exit_button()
    ################################################################################################
    receive_data = serial1.read()
    if receive_data:
        receive_data_str = receive_data.decode("utf-8")
        if receive_data_str == '2':
            mode = 2
        if receive_data_str == '3':
            mode = 3      
        if receive_data_str == '4':
            mode = 4                 
    # print(f"数据类型：{type(receive_data)}，接收的数据{receive_data}")
    # print(type(receive_data.decode))
    print("模式为：", mode)
    ################################################################################################
    if binary_flag == 1:
        img_binary = img.binary(threshold_blue)
        disp.show(img_binary)
    screen_lab() # 串口屏调整阈值
    ################################################################################################
    # if rect_flag == True:
    #     img_copy.draw_rect(coners_x[0],coners_y[0],coners_x[1] - coners_x[0], coners_y[0] - coners_y[3], image.COLOR_PURPLE,thickness=2)
    #     print(coners_x, coners_y)
    #     print(coners_x[0],coners_y[0],coners_x[1] - coners_x[0], coners_y[3] - coners_y[0])
    if rect_flag2 == True:
        blobs_jxb_in = img.find_blobs([threshold_jxb_in], roi=[coners_x[0],coners_y[0],coners_x[1] - coners_x[0], coners_y[3] - coners_y[0]], x_stride=2, y_stride=1, area_threshold=area_threshold2, pixels_threshold=pixels_threshold2)
        blobs_jxb_out = img.find_blobs([threshold_jxb_out], roi=[1,1,img.width()-1, img.height()-1], x_stride=2, y_stride=1, area_threshold=area_threshold2, pixels_threshold=pixels_threshold2)

        print(f"模式二下发送机械臂的x坐标为；{jxb_x}，y坐标为：{jxb_y}") # 调试机械臂坐标
        if blobs_jxb_in:
            max_ID=[-1,-1]
            max_ID=find_max(blobs_jxb_in)  #找最大色块       
            img_copy.draw_rect(blobs_jxb_in[max_ID[0]][0],blobs_jxb_in[max_ID[0]][1],blobs_jxb_in[max_ID[0]][2], blobs_jxb_in[max_ID[0]][3], image.COLOR_GREEN)
            img_copy.draw_cross(blobs_jxb_in[max_ID[0]].cx(),blobs_jxb_in[max_ID[0]].cy(),image.COLOR_GREEN)
            XL_in=blobs_jxb_in[max_ID[0]].cx() # 机械臂x中心坐标
            XR_in=XL_in
            YU_in=blobs_jxb_in[max_ID[1]].cy() # 机械臂y中心坐标
            YD_in=YU_in
            img_copy.draw_string(80, 8, str(jxb_x), image.COLOR_WHITE)
            img_copy.draw_string(100, 8, str(jxb_y), image.COLOR_WHITE)
            img_copy.draw_string(80, 30, str(blobs_jxb_in[max_ID[0]].pixels()), image.COLOR_WHITE)
            if max_ID[1]!=-1:
                img_copy.draw_rect(blobs_jxb_in[max_ID[1]][0],blobs_jxb_in[max_ID[1]][1],blobs_jxb_in[max_ID[1]][2], blobs_jxb_in[max_ID[1]][3], image.COLOR_GREEN)
                img_copy.draw_cross(blobs_jxb_in[max_ID[1]].cx(),blobs_jxb_in[max_ID[1]].cy(),image.COLOR_GREEN)
                if(blobs_jxb_in[max_ID[0]].cx()>blobs_jxb_in[max_ID[1]].cx()):
                    XL_in=blobs_jxb_in[max_ID[0]].cx()
                    XR_in=blobs_jxb_in[max_ID[1]].cx()
                else:
                    XL_in=blobs_jxb_in[max_ID[1]].cx()
                    XR_in=blobs_jxb_in[max_ID[0]].cx()
                if(blobs_jxb_in[max_ID[0]].cy()>blobs_jxb_in[max_ID[1]].cy()):
                    YU_in=blobs_jxb_in[max_ID[0]].cy()
                    YD_in=blobs_jxb_in[max_ID[1]].cy()
                else:
                    YU_in=blobs_jxb_in[max_ID[1]].cy()
                    YD_in=blobs_jxb_in[max_ID[0]].cy()
            jxb_x = XL_in
            jxb_y = YU_in

        elif blobs_jxb_out:
            max_ID=[-1,-1]
            max_ID=find_max(blobs_jxb_out)  #找最大色块       
            img_copy.draw_rect(blobs_jxb_out[max_ID[0]][0],blobs_jxb_out[max_ID[0]][1],blobs_jxb_out[max_ID[0]][2], blobs_jxb_out[max_ID[0]][3], image.COLOR_GREEN)
            img_copy.draw_cross(blobs_jxb_out[max_ID[0]].cx(),blobs_jxb_out[max_ID[0]].cy(),image.COLOR_GREEN)
            XL_out=blobs_jxb_out[max_ID[0]].cx() # 机械臂x中心坐标
            XR_out=XL_out
            YU_out=blobs_jxb_out[max_ID[1]].cy() # 机械臂y中心坐标
            YD_out=YU_out
            img_copy.draw_string(80, 8, str(jxb_x), image.COLOR_WHITE)
            img_copy.draw_string(100, 8, str(jxb_y), image.COLOR_WHITE)
            img_copy.draw_string(80, 30, str(blobs_jxb_out[max_ID[0]].pixels()), image.COLOR_WHITE)
            if max_ID[1]!=-1:
                img_copy.draw_rect(blobs_jxb_out[max_ID[1]][0],blobs_jxb_out[max_ID[1]][1],blobs_jxb_out[max_ID[1]][2], blobs_jxb_out[max_ID[1]][3], image.COLOR_GREEN)
                img_copy.draw_cross(blobs_jxb_out[max_ID[1]].cx(),blobs_jxb_out[max_ID[1]].cy(),image.COLOR_GREEN)
                if(blobs_jxb_out[max_ID[0]].cx()>blobs_jxb_out[max_ID[1]].cx()):
                    XL_out=blobs_jxb_out[max_ID[0]].cx()
                    XR_out=blobs_jxb_out[max_ID[1]].cx()
                else:
                    XL_out=blobs_jxb_out[max_ID[1]].cx()
                    XR_out=blobs_jxb_out[max_ID[0]].cx()
                if(blobs_jxb_out[max_ID[0]].cy()>blobs_jxb_out[max_ID[1]].cy()):
                    YU_out=blobs_jxb_out[max_ID[0]].cy()
                    YD_out=blobs_jxb_out[max_ID[1]].cy()
                else:
                    YU_out=blobs_jxb_out[max_ID[1]].cy()
                    YD_out=blobs_jxb_out[max_ID[0]].cy()   
            jxb_x = XL_out
            jxb_x = YU_out


        # elif blobs_jxb_in and blobs_jxb_out:
        #     max_index_in = find_max_blob(blobs_jxb_in)
        #     max_blob_in = blobs_jxb_in[max_index_in]
        #     max_index_out = find_max_blob(blobs_jxb_out)
        #     max_blob_out = blobs_jxb_out[max_index_out]

        #     # 计算两个ROI内最大色块的平均中心坐标
        #     avg_X = (max_blob_in.cx() + max_blob_out.cx()) / 2
        #     avg_Y = (max_blob_in.cy() + max_blob_out.cy()) / 2

        #     img_copy.draw_rect(max_blob_in[0], max_blob_in[1], max_blob_in[2], max_blob_in[3], image.COLOR_GREEN)
        #     img_copy.draw_cross(max_blob_in.cx(), max_blob_in.cy(), image.COLOR_GREEN)
        #     img_copy.draw_string(80, 8, str(avg_X), image.COLOR_WHITE)
        #     img_copy.draw_string(100, 8, str(avg_Y), image.COLOR_WHITE)
        #     img_copy.draw_string(80, 30, str(max_blob_in.pixels()), image.COLOR_WHITE)    

        #     print(f"模式二下发送机械臂的x坐标为；{avg_X}，y坐标为：{avg_Y}") # 调试机械臂坐标
                                   
                    
    ################################################################################################
    # mode = 1
    if mode == 1:
        if white_flag2 == False:
            blobs1 = img.find_blobs(threshold_white, roi=[1,1,img.width()-1, img.height()-1], x_stride=2, y_stride=1, area_threshold=area_threshold2, pixels_threshold=pixels_threshold2)
            if len(blobs1) == 5:
                for i in range(5):
                    # 过滤掉棋盘
                    if blobs1[i].w() > 55 and blobs1[i].h() > 55:
                        continue
                    if blobs1[i].w() < 20 and blobs1[i].h() < 20:
                        continue
                    aspect_ratio = blobs1[i].w() / blobs1[i].h()
                    if aspect_ratio < 0.6 or aspect_ratio > 1.4:  # 假设棋子的宽高比接近1
                        continue

                    x1[i], y1[i], r1[i] = blobs1[i].enclosing_circle() 

                    img_copy.draw_circle(x1[i], y1[i], r1[i], image.COLOR_GREEN, 2)
                    img_copy.draw_cross(x1[i], y1[i], image.COLOR_GREEN,thickness=2)

                    if abs(y1[i] - last_y1[i] < 3):
                        count_white += 1
                    else:
                        count_white = 0
                        print("识别白棋子不稳定，重新识别")
                    if count_white >= 50:
                        valid_flag1 = True
                    if valid_flag1 == True:
                        for i in range(5):
                            white_circle[i] = (x1[i], y1[i])  # 添加到列表中
                            white_circle = sorted(white_circle, key=lambda coord: coord[1])
                        if all(point != (0, 0) for point in white_circle):
                            white_flag2 = True
                            print(f"白棋坐标：{white_circle}")
                        else:
                            white_flag2 = False                            
                    for j in range(5):
                        last_y1[j] = y1[j]
        # 黑棋子的寻找roi，需要设定，因机械臂是黑色的
        img_copy.draw_rect(1,15,img.width()-1, img.height()-1, image.COLOR_RED,thickness=2)
        if black_flag2 == False:
            blobs2 = img.find_blobs(threshold_black, roi=[1,15,img.width()-1, img.height()-1], x_stride=2, y_stride=1, area_threshold=area_threshold2, pixels_threshold=pixels_threshold2)
            # 过滤掉棋盘
            for b2 in blobs2:
                if b2.w() > 55 and b2.h() > 55:
                    continue
                if b2.w() < 20 and b2.h() < 20:
                    continue
                aspect_ratio = b2.w() / b2.h()
                if aspect_ratio < 0.6 or aspect_ratio > 1.4:  # 假设棋子的宽高比接近1
                    continue            
                if len(blobs2) == 5:
                    for i in range(5):
                        x2[i], y2[i], r2[i] = blobs2[i].enclosing_circle() 

                        img_copy.draw_circle(x2[i], y2[i], r2[i], image.COLOR_GREEN, 2)
                        img_copy.draw_cross(x2[i], y2[i], image.COLOR_GREEN,thickness=2)

                        if abs(y2[i] - last_y2[i] < 3):
                            count_black += 1
                            print(count_black)
                        else:
                            count_black = 0
                            print("识别黑棋子不稳定，重新识别")
                        if count_black >= 50:
                            valid_flag2 = True
                        if valid_flag2 == True:
                            for i in range(5):
                                black_circle[i] = (x2[i], y2[i])  # 添加到列表中
                                black_circle = sorted(black_circle, key=lambda coord: coord[1])
                            if all(point != (0, 0) for point in black_circle):
                                black_flag2 = True
                                print(f"黑棋坐标：{black_circle}")
                            else:
                                black_flag2 = False
                        for j in range(5):
                            last_y2[j] = y2[j]

        for white in white_circle:
            img_copy.draw_cross(white[0], white[1],image.COLOR_PURPLE,thickness=2)
        for black in black_circle:
            img_copy.draw_cross(black[0], black[1],image.COLOR_PURPLE,thickness=2)
        # 提取坐标
        white_x1 = [white[0] for white in white_circle]
        white_y1 = [white[1] for white in white_circle]
        black_x1 = [black[0] for black in black_circle]
        black_y1 = [black[1] for black in black_circle]
    ################################################################################################
        # 找棋盘
        if rect_flag == False:
            img_cv = image.image2cv(img, False, False)
            gray = cv2.cvtColor(img_cv, cv2.COLOR_RGB2GRAY)

            # 高斯模糊去噪声
            blurred = cv2.GaussianBlur(gray, (5, 5), 0)
            # Bila = cv2.bilateralFilter(gray, 5, 100, 150) # 双边滤波
            # 边缘检测，阈值 0，150
            edged = cv2.Canny(blurred, 50, 150)

            # 闭运算
            kernel = np.ones((5, 5), np.uint8)
            closing = cv2.morphologyEx(edged, cv2.MORPH_CLOSE, kernel)
            # kernel = np.ones((3, 3), np.uint8)
            # dilated = cv2.dilate(edged, kernel, iterations=1)
            edges_close = cv2.morphologyEx(closing, cv2.MORPH_CLOSE, kernel)

            # disp.show(image.cv2image(edges_close, False, False))
            # 查找轮廓
            contours, _ = cv2.findContours(edges_close, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
            if len(contours) > 0:
                # 筛选出最大的轮廓
                largest_contour = max(contours, key=cv2.contourArea)

                # 近似多边形
                epsilon = 0.02 * cv2.arcLength(largest_contour, True)
                approx = cv2.approxPolyDP(largest_contour, epsilon, True)

                # 如果找到的是一个四边形
                if len(approx) == 4:
                    # print("rect found")
                    corners = approx.reshape((4, 2))
                    # 按顺序排列角点（左上、右上、右下、左下）
                    rect = np.zeros((4, 2), dtype="int")
                    s = corners.sum(axis=1)
                    rect[0] = corners[np.argmin(s)]
                    rect[2] = corners[np.argmax(s)]
                    diff = np.diff(corners, axis=1)
                    rect[1] = corners[np.argmin(diff)]
                    rect[3] = corners[np.argmax(diff)]
                    corners = rect

                    # TODO: 这是外角点，可以优化一下找到线中心的点，这里直接写死往内收拢 1 个像素
                    half_x = 1
                    half_y = 1
                    corners[0] += (half_x, half_y)
                    corners[1] += (-half_x, half_y)
                    corners[2] += (-half_x, -half_y)
                    corners[3] += (half_x, -half_y)

                    # 绘制顶点和轮廓
                    for corner in corners:
                        cv2.circle(img_cv, tuple(corner), 4, (0, 255, 0), -1)

                    coners_list = corners.tolist()
                    coners_x = [my_coners[0] for my_coners in coners_list]
                    coners_y = [my_coners[1] for my_coners in coners_list]

                    # 绘制四边路径性
                    # cv2.drawContours(img_cv, [approx], -1, (0, 255, 0), 1)

                    # 洪泛填充外部，如果棋盘外部的背景和棋盘内部的背景相同且后面用了找色块的方式找才需要这一步
                    if find_center_method == 2:
                        img.flood_fill(corners[0][0] - 5, corners[0][1] - 5, 0.3, 0.3, image.COLOR_BLUE)
                        # img.flood_fill(corners[1][0] - 5, corners[1][1] + 5, 0.5, 0.05, image.COLOR_BLUE)
                        # img.flood_fill(corners[0][0] + 5, corners[0][1] + 5, 0.5, 0.05, image.COLOR_BLUE)
                        # img.flood_fill(corners[0][0] + 5, corners[0][1] - 5, 0.5, 0.05, image.COLOR_BLUE)

                    centers = []
                    # 找格子中心点方法一：直接根据顶点计算
                    if find_center_method == 1:
                        # 根据顶点找出中心点
                        centers  = find_grid_centers(corners)
                        for i in range(9):
                            for j in range(2):
                                if abs(centers[i][j] - last_centers[i][j] < 3):
                                    count += 1
                                    # print(count)
                                else:
                                    count = 0
                                    print("识别有误！")       
                        if count >= 50:
                            print("识别成功！")
                            rect_flag = True # 模式一下准确识别50次棋盘后不识别棋盘，释放空间
                            rect_flag2 = True #发送数据
                            for i in range(9):
                                print(f"第{i}个方格，x：{centers[i][0]}, y：{centers[i][1]}")
                            count = 0
                        rect_x = [my_rect[0] for my_rect in centers]
                        rect_y = [my_rect[1] for my_rect in centers]
                        for i in range(9):
                            last_centers[i] = centers[i]
                    # 找格子中心点方法二： 找色块的方式来确定中心点
                    elif find_center_method == 2:                # 上面出来的结果已经是 点从左上顺时针
                        roi = [corners[:,0].min(), corners[:,1].min(), corners[:,0].max() - corners[:,0].min(), corners[:,1].max() - corners[:,1].min()]
                        img.draw_rect(roi[0], roi[1], roi[2], roi[3], image.COLOR_WHITE)
                        blobs = img.find_blobs(thresholds, roi=roi, x_stride=2, y_stride=1, area_threshold=area_threshold, pixels_threshold=pixels_threshold)
                        for b in blobs:
                            centers.append((b.cx(), b.cy()))
                                        # 对找到的中心点进行编号, y + x 最大就是右下角，最小就是左上角， y-x 最大就是左下角，y-x 最小就是右上角，其它几个点根据在旁边两个点中间判断
                        if len(centers) == 9:
                            centers = np.array(centers)
                            rect = np.zeros((9, 2), dtype="int")
                            s = centers.sum(axis=1)
                            idx_0 = np.argmin(s)
                            idx_8 = np.argmax(s)
                            diff = np.diff(centers, axis=1)
                            idx_2 = np.argmin(diff)
                            idx_6 = np.argmax(diff)
                            rect[0] = centers[idx_0]
                            rect[2] = centers[idx_2]
                            rect[6] = centers[idx_6]
                            rect[8] = centers[idx_8]
                            #   其它点
                            calc_center = (rect[0] + rect[2] + rect[6] + rect[8]) / 4
                            mask = np.zeros(centers.shape[0], dtype=bool)
                            idxes = [1, 3, 4, 5, 7]
                            mask[idxes] = True
                            others = centers[mask]
                            idx_l = others[:,0].argmin()
                            idx_r = others[:,0].argmax()
                            idx_t = others[:,1].argmin()
                            idx_b = others[:,1].argmax()
                            found = np.array([idx_l, idx_r, idx_t, idx_b])
                            mask = np.isin(range(len(others)), found, invert=False)
                            idx_c = np.where(mask == False)[0]
                            if len(idx_c) == 1:
                                rect[1] = others[idx_t]
                                rect[3] = others[idx_l]
                                rect[4] = others[idx_c]
                                rect[5] = others[idx_r]
                                rect[7] = others[idx_b]
                            else:
                                # 等于 45度的情况
                                print("== 45 degree")
                            centers = rect
                    else:
                        raise Exception("find_center_method value error")

                    # 画出中心点
                    # 写编号
                    if len(centers) == 9:
                        for i in range(9):
                            # x, y = centers[i][0], centers[i][1]
                            x, y = rect_x[i], rect_y[i]
                            cv2.circle(img_cv, (x, y), 2, (0, 255, 0), -1)
                            img_copy.draw_string(x, y, f"{i + 1}", image.COLOR_WHITE, scale=2, thickness=-1)
    ################################################################################################
    # mode3 转45度找棋盘
    if mode == 3:
        # 找棋盘
        if rect_flag_mode3 == False:
            img_cv = image.image2cv(img, False, False)
            gray = cv2.cvtColor(img_cv, cv2.COLOR_RGB2GRAY)

            # 高斯模糊去噪声
            blurred = cv2.GaussianBlur(gray, (5, 5), 0)
            # Bila = cv2.bilateralFilter(gray, 5, 100, 150) # 双边滤波
            # 边缘检测，阈值 0，150
            edged = cv2.Canny(blurred, 50, 150)

            # 闭运算
            kernel = np.ones((5, 5), np.uint8)
            closing = cv2.morphologyEx(edged, cv2.MORPH_CLOSE, kernel)
            # kernel = np.ones((3, 3), np.uint8)
            # dilated = cv2.dilate(edged, kernel, iterations=1)
            edges_close = cv2.morphologyEx(closing, cv2.MORPH_CLOSE, kernel)

            # disp.show(image.cv2image(edges_close, False, False))
            # 查找轮廓
            contours, _ = cv2.findContours(edges_close, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
            if len(contours) > 0:
                # 筛选出最大的轮廓
                largest_contour = max(contours, key=cv2.contourArea)

                # 近似多边形
                epsilon = 0.02 * cv2.arcLength(largest_contour, True)
                approx = cv2.approxPolyDP(largest_contour, epsilon, True)

                # 如果找到的是一个四边形
                if len(approx) == 4:
                    # print("rect found")
                    corners = approx.reshape((4, 2))
                    # 按顺序排列角点（左上、右上、右下、左下）
                    rect = np.zeros((4, 2), dtype="int")
                    s = corners.sum(axis=1)
                    rect[0] = corners[np.argmin(s)]
                    rect[2] = corners[np.argmax(s)]
                    diff = np.diff(corners, axis=1)
                    rect[1] = corners[np.argmin(diff)]
                    rect[3] = corners[np.argmax(diff)]
                    corners = rect

                    # TODO: 这是外角点，可以优化一下找到线中心的点，这里直接写死往内收拢 1 个像素
                    half_x = 1
                    half_y = 1
                    corners[0] += (half_x, half_y)
                    corners[1] += (-half_x, half_y)
                    corners[2] += (-half_x, -half_y)
                    corners[3] += (half_x, -half_y)

                    # 绘制顶点和轮廓
                    for corner in corners:
                        cv2.circle(img_cv, tuple(corner), 4, (0, 255, 0), -1)

                    # 绘制四边路径性
                    # cv2.drawContours(img_cv, [approx], -1, (0, 255, 0), 1)

                    # 洪泛填充外部，如果棋盘外部的背景和棋盘内部的背景相同且后面用了找色块的方式找才需要这一步
                    if find_center_method == 2:
                        img.flood_fill(corners[0][0] - 5, corners[0][1] - 5, 0.3, 0.3, image.COLOR_BLUE)
                        # img.flood_fill(corners[1][0] - 5, corners[1][1] + 5, 0.5, 0.05, image.COLOR_BLUE)
                        # img.flood_fill(corners[0][0] + 5, corners[0][1] + 5, 0.5, 0.05, image.COLOR_BLUE)
                        # img.flood_fill(corners[0][0] + 5, corners[0][1] - 5, 0.5, 0.05, image.COLOR_BLUE)

                    centers = []
                    # 找格子中心点方法一：直接根据顶点计算
                    if find_center_method == 1:
                        # 根据顶点找出中心点
                        centers  = find_grid_centers(corners)
                        for i in range(9):
                            for j in range(2):
                                if abs(centers[i][j] - last_centers[i][j] < 3):
                                    count += 1
                                    # print(count)
                                else:
                                    count = 0
                                    print("识别有误！")       
                        if count >= 50:
                            print("识别成功！")
                            # rect_flag = True # 模式三下准确识别50次棋盘后不识别棋盘，释放空间
                            rect_flag2 = True #发送数据
                            count = 0
                        rect_x_mode3 = [my_rect[0] for my_rect in centers]
                        rect_y_mode3 = [my_rect[1] for my_rect in centers]
                        for i in range(9):
                            last_centers[i] = centers[i]
                    else:
                        raise Exception("find_center_method value error")

                    # 画出中心点
                    # 写编号
                    if len(centers) == 9:
                        for i in range(9):
                            # x, y = centers[i][0], centers[i][1]
                            x, y = rect_x_mode3[i], rect_y_mode3[i]
                            cv2.circle(img_cv, (x, y), 2, (0, 255, 0), -1)
                            img_copy.draw_string(x, y, f"{i + 1}", image.COLOR_WHITE, scale=2, thickness=-1)
    ################################################################################################

    ################################################################################################
    if mode == 1:
        if task1_uart_flag == False: # 发送后只发一次
            if black_flag2 and white_flag2 and rect_flag2:
            # if rect_flag2:
                # # 21个H，18个f，两个B，个字节
                # data1 = struct.pack('>HHHHHHHHHHHHHHHHHHHHHffffffffffffffffffBB', mode, white_x1[0], white_x1[1], white_x1[2], white_x1[3], white_x1[4], white_y1[0], white_y1[1], white_y1[2], white_y1[3], white_y1[4], black_x1[0], black_x1[1], black_x1[2], black_x1[3], black_x1[4], black_y1[0], black_y1[1], black_y1[2], black_y1[3], black_y1[4], rect_x[0], rect_x[1], rect_x[2], rect_x[3], rect_x[4], rect_x[5], rect_x[6], rect_x[7], rect_x[8], rect_y[0], rect_y[1], rect_y[2], rect_y[3], rect_y[4], rect_y[5], rect_y[6], rect_y[7], rect_y[8], 0x0D, 0x0A)
                # 39个H，两个B，80个字节 初始化发送的坐标
                data1 = struct.pack('>HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHBB', mode, white_x1[0], white_x1[1], white_x1[2], white_x1[3], white_x1[4], white_y1[0], white_y1[1], white_y1[2], white_y1[3], white_y1[4], black_x1[0], black_x1[1], black_x1[2], black_x1[3], black_x1[4], black_y1[0], black_y1[1], black_y1[2], black_y1[3], black_y1[4], rect_x[0], rect_x[1], rect_x[2], rect_x[3], rect_x[4], rect_x[5], rect_x[6], rect_x[7], rect_x[8], rect_y[0], rect_y[1], rect_y[2], rect_y[3], rect_y[4], rect_y[5], rect_y[6], rect_y[7], rect_y[8], 0x0D, 0x0A)
                # 39个H，两个B，80个字节
                # data1 = struct.pack('>HHHHHHHHHHHHHHHHHHHBB', mode, rect_x[0], rect_x[1], rect_x[2], rect_x[3], rect_x[4], rect_x[5], rect_x[6], rect_x[7], rect_x[8], rect_y[0], rect_y[1], rect_y[2], rect_y[3], rect_y[4], rect_y[5], rect_y[6], rect_y[7], rect_y[8], 0x0D, 0x0A)
                serial1.write(data1)
                task1_uart_flag = True
    elif mode == 2:
        # 3个H，两个B，8个字节 机械臂的二维坐标
        data2 = struct.pack('>HHHBB', 2, XL, YU, 0x0D, 0x0A)
        serial1.write(data2)
        print(f"模式二下发送机械臂的x坐标为；{XL}，y坐标为：{YU}") # 调试机械臂坐标
    elif mode == 3:
        # 19个H， 两个B，40个字节 
        data3 = struct.pack('>HHHHHHHHHHHHHHHHHHHBB', 3, rect_x_mode3[0], rect_x_mode3[1], rect_x_mode3[2], rect_x_mode3[3], rect_x_mode3[4], rect_x_mode3[5], rect_x_mode3[6], rect_x_mode3[7], rect_x_mode3[8], rect_y_mode3[0], rect_y_mode3[1], rect_y_mode3[2], rect_y_mode3[3], rect_y_mode3[4], rect_y_mode3[5], rect_y_mode3[6], rect_y_mode3[7], rect_y_mode3[8], 0x0D, 0X0A)
        serial1.write(data3)
    ################################################################################################
    # img_show = image.cv2image(edged)
    disp.show(img_copy)
    print(f"time: {time.ticks_ms() - t}ms, fps: {1000 / (time.ticks_ms() - t)}")
    # del cam
    gc.collect() # 回收垃圾